#include "motor.h"
#include "common.h"
#include "interface.h"
#include <stdint.h>

// GPIO配置函数
void MotorGPIO_Configuration(void) {
  GPIO_InitTypeDef GPIO_InitStructure;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
#ifndef CUSTOM_PWM_ENABLED
  GPIO_InitStructure.GPIO_Pin = FRONT_LEFT_F_PIN;
  GPIO_Init(FRONT_LEFT_F_GPIO, &GPIO_InitStructure);

  GPIO_InitStructure.GPIO_Pin = FRONT_LEFT_B_PIN;
  GPIO_Init(FRONT_LEFT_B_GPIO, &GPIO_InitStructure);

  GPIO_InitStructure.GPIO_Pin = FRONT_RIGHT_F_PIN;
  GPIO_Init(FRONT_RIGHT_F_GPIO, &GPIO_InitStructure);

  GPIO_InitStructure.GPIO_Pin = FRONT_RIGHT_B_PIN;
  GPIO_Init(FRONT_RIGHT_B_GPIO, &GPIO_InitStructure);

  GPIO_InitStructure.GPIO_Pin = BEHIND_LEFT_F_PIN;
  GPIO_Init(BEHIND_LEFT_F_GPIO, &GPIO_InitStructure);

  GPIO_InitStructure.GPIO_Pin = BEHIND_LEFT_B_PIN;
  GPIO_Init(BEHIND_LEFT_B_GPIO, &GPIO_InitStructure);
#endif
  GPIO_InitStructure.GPIO_Pin = BEHIND_RIGHT_F_PIN;
  GPIO_Init(BEHIND_RIGHT_F_GPIO, &GPIO_InitStructure);

  GPIO_InitStructure.GPIO_Pin = BEHIND_RIGHT_B_PIN;
  GPIO_Init(BEHIND_RIGHT_B_GPIO, &GPIO_InitStructure);
}

#ifndef CUSTOM_PWM_ENABLED

void CarMove(void) {

  BEHIND_RIGHT_EN;

  // 右轮
  if (front_right_speed_duty > 0) {
    if (speed_count < front_right_speed_duty) {
      FRONT_RIGHT_GO; // 向前
    } else {
      FRONT_RIGHT_STOP; // 停止
    }
  } else if (front_right_speed_duty < 0) {
    if (speed_count < (-1) * front_right_speed_duty) {
      FRONT_RIGHT_BACK; // 向后
    } else {
      FRONT_RIGHT_STOP; // 停止
    }
  } else {
    FRONT_RIGHT_STOP; // 停止
  }

  // 左轮
  if (behind_left_speed_duty > 0) {
    if (speed_count < behind_left_speed_duty) {
      BEHIND_LEFT_GO; // 向前
    } else {
      BEHIND_LEFT_STOP; // 停止
    }
  } else if (behind_left_speed_duty < 0) {
    if (speed_count < (-1) * behind_left_speed_duty) {
      BEHIND_LEFT_BACK; // 向后
    } else {
      BEHIND_LEFT_STOP; // 停止
    }
  } else {
    BEHIND_LEFT_STOP; // 停止
  }
}

// 向前
void CarGo(void) {
  front_right_speed_duty = 50; // 右前进
  behind_left_speed_duty = 50; // 左前进
}

// 后退
void CarBack(void) {
  front_right_speed_duty = -30; // 右前进
  behind_left_speed_duty = -30; // 左前进
}

// 向左
void CarLeft(void) {
  front_right_speed_duty = 40;  // 右前进
  behind_left_speed_duty = -30; // 左前进
}

// 向右
void CarRight(void) {
  front_right_speed_duty = -30; // 右前进
  behind_left_speed_duty = 40;  // 左前进
}

// 停止
void CarStop(void) {
  front_right_speed_duty = 0; // 右前进
  behind_left_speed_duty = 0; // 左前进
}

void MotorInit(void) {
  MotorGPIO_Configuration();
  CarStop();
}

#else
#include "PID.h"
#include "manualPWM.h"

static PWM motorRR,               // 右轮
    motorLL;                      // 左轮
static uint32_t scalerTo10ms = 1; // 10ms分频
// static char useManualDuty = 0;    // 0:自动控制 1:手动控制
extern PID DirCtrl,       // 姿态PID
    SpeedCtrl;            // 速度PID
extern char redrayStatus; // 红外避障状态
static uint32_t innerTick = 0;

static inline uint16_t _TIM_GetAutoreload(TIM_TypeDef *timAddr) {
  assert_param(IS_TIM_ALL_PERIPH(timAddr));
  return (timAddr->ARR + 1);
}

void MotorInit(void) {
  MotorGPIO_Configuration();
  PWM_Init(&motorRR);
  PWM_Init(&motorLL);

  // 由于接线，这里左右侧定义反了，反过来配置
  PWM_Config(&motorRR, 1000, PWM_Unipolar, TIM2,
             MOTOR_GPIO_PORT_L_SIDE_POSITIVE, MOTOR_GPIO_PORT_L_SIDE_NEGATIVE,
             MOTOR_GPIO_PIN_L_SIDE_POSITIVE, MOTOR_GPIO_PIN_L_SIDE_NEGATIVE);
  PWM_Config(&motorLL, 1000, PWM_Unipolar, TIM2,
             MOTOR_GPIO_PORT_R_SIDE_POSITIVE, MOTOR_GPIO_PORT_R_SIDE_NEGATIVE,
             MOTOR_GPIO_PIN_R_SIDE_POSITIVE, MOTOR_GPIO_PIN_R_SIDE_NEGATIVE);
#ifdef TEST_MOTOR
  // motorL._TIMReloadCount = 500;
  // motorL._TIMReloadCount = 500;
  PWM_SetDuty(&motorLL, 0);
  PWM_SetDuty(&motorRR, 0);
#else
  PWM_SetDuty(&motorLL, 0);
  PWM_SetDuty(&motorRR, 0);
  scalerTo10ms = 10 * 1000 / TIM_DEFAULT_PERIOD_us / _TIM_GetAutoreload(TIM2);
#endif
}

static inline void _inn_SetDuty(int16_t dutyL, int16_t dutyR) {
  PWM_SetDuty(&motorRR, dutyL);
  PWM_SetDuty(&motorLL, dutyR);
}

// void setDuty(int16_t dutyL, int16_t dutyR) {
//   _inn_SetDuty(dutyL, dutyR);
//   useManualDuty = 1;
// }

void getDuty(int16_t *dutyL, int16_t *dutyR) {
  *dutyL = motorLL.duty_pct;
  *dutyR = motorLL.duty_pct;
}

// void dutyHoldOn(void) { useManualDuty = 1; }
// void dutyHoldOff(void) { useManualDuty = 0; }

/**
 * @brief Set motor duty by PID output.
 *
 * This function is supposed to be called every 10ms.
 * It gets the output of SpeedCtrl and DirCtrl, and sets the duty of motorL and
 * motorR accordingly. If useManualDuty is true, don't change the duty.
 */
void PID2Motor(void) {
  innerTick++;
  if (innerTick >= scalerTo10ms) {
    innerTick = 0;
    int speed = PID_GetOutput(&SpeedCtrl);  // assert speed within [-100, 100]
    int turn = PID_GetOutput(&DirCtrl) / 2; // assert turn within [-200, 200]
    int left_duty = speed + turn;
    int right_duty = speed - turn;
    if (left_duty > 100) {
      right_duty -= left_duty - 100;
      left_duty = 100;
    } else if (right_duty > 100) {
      left_duty -= right_duty - 100;
      right_duty = 100;
    } else if (left_duty < -100) {
      right_duty -= left_duty + 100;
      left_duty = -100;
    } else if (right_duty < -100) {
      left_duty -= right_duty + 100;
      right_duty = -100;
    }
    _inn_SetDuty(left_duty, right_duty);
  }
}

/**
 * @brief Motor control function
 * @details This function is called by the TIM interrupt to control the
 * motor. It enables the FETs for the motors and then steps the PWMs for
 * them.
 */
void CarMove(void) {
  // Enable the FETs
  // Forgive for these names for no reason
  GPIO_SetBits(BEHIND_RIGHT_F_GPIO, BEHIND_RIGHT_F_PIN);
  GPIO_SetBits(BEHIND_RIGHT_B_GPIO, BEHIND_RIGHT_B_PIN);
  PWM_Step(&motorRR);
  PWM_Step(&motorLL);
}

void assignSpeed(int speed) { PID_SetOutput(&SpeedCtrl, speed); }
/**
 * @brief Set rotate speed for car.
 * @details Input speed refers to the delta between left and right wheels duty.
 * Positive value turns left. Limited to [-200, 200]
 * @param speed The input rotate speed.
 */
void assignRotateSpeed(int speed) {
  speed = max(-200, speed);
  speed = min(200, speed);
  PID_SetOutput(&DirCtrl, speed);
}
void PIDFreeze(void) {
  PID_HoldOn(&SpeedCtrl);
  PID_HoldOn(&DirCtrl);
}
void PIDRelease(void) {
  PID_HoldOff(&DirCtrl);
  // 注意：由于速度控制本来就是手动控制，因此反而不能切到自动，相反要丢到速度设计值（100）
  // PID_HoldOff(&SpeedCtrl);
  PID_SetOutput(&SpeedCtrl, 100);
}
#endif
